#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <chrono>
#include "turtlesim/msg/pose.hpp"

using namespace std::chrono_literals;

class TurtleControlNode : public rclcpp::Node
{
private:
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscriber;
double targe_x{1.0};
double targe_y{1.0};
double k{1.0};
double max_speed{3.0};
public:
    explicit TurtleControlNode(const std::string &node_name) : Node(node_name)
    {
        publisher = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);
        subscriber = this->create_subscription<turtlesim::msg::Pose>("turtle1/pose",10,std::bind(&TurtleControlNode::on_pose_received, this,std::placeholders::_1));
    }
  void on_pose_received(const turtlesim::msg::Pose::SharedPtr pose)
    {

      auto current_x = pose->x;
      auto current_y = pose->y;
      RCLCPP_INFO(get_logger(), "当前: x=%f, y=%f", current_x, current_y);

      auto dist = std::sqrt(
            (targe_x - current_x)*(targe_x - current_x)+(targe_y - current_y)*(targe_y - current_y));
      auto angle = std::atan2((targe_y - current_y), (targe_x - current_x)) - pose->theta;

      auto msg = geometry_msgs::msg::Twist();
         if(dist>0.1)
        {

         if(fabs(angle)>0.2)
            {
               msg.angular.z = fabs(angle);
            }

         else{msg.linear.x = k*dist;}
        }
         if(msg.linear.x> max_speed)
   {
    msg.linear.x=max_speed;
   }
   publisher->publish(msg);
   }
};
int main(int argc, char **argv)
{
    rclcpp::init(argc,argv);
auto node = std::make_shared<TurtleControlNode>("turtle_control");
    rclcpp::spin(node);
    rclcpp::shutdown();
return 0;
}